#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy, sys
from time import sleep
from sg805_driver.msg import ExtraFeatures

    #根据功能编号进行调用，参考 sg805RobotRos中 extraFeaturesCB回调函数的内容。
    # extraMsg.Tag = 8
    # PIN0_ON = 8,            //GPIO_0高电平
    # PIN0_OFF = 9,           //GPIO_0低电平
    # PIN1_ON = 10,            //GPIO_1高电平
    # PIN1_OFF = 11,           //GPIO_1低电平
    
class ExtraFeaturesDemo():
    def __init__(self):
        rospy.init_node('extraFeaturesCommander', anonymous = False )
        self.pub = rospy.Publisher("/sg805/extraFeatures", ExtraFeatures, queue_size=10)
        self.extraMsg = ExtraFeatures()
        sleep(2)

        
        while(not rospy.is_shutdown()):
            self.attach()
            sleep(5)
            sleep.release()
            sleep(5)


    # 夹取
    def attach(self):
        # 气阀IO关闭
        self.extraMsg.Tag = 11
        self.pub.publish(self.extraMsg)
        sleep(0.5)
        # 气泵IO打开
        self.extraMsg.Tag = 8
        self.pub.publish(self.extraMsg)
        sleep(1.5)

    # 释放
    def release(self):
        # 气泵IO关闭
        self.extraMsg.Tag = 9
        self.pub.publish(self.extraMsg)
        sleep(0.5)
        # 气阀IO打开
        self.extraMsg.Tag = 10
        self.pub.publish(self.extraMsg)
        sleep(1.5)


if __name__ == '__main__':
    try:
        ExtraFeaturesDemo()
    except rospy.ROSInterruptException:
        pass